#!/usr/bin/env python
# -*- coding: utf-8 -*-
# 该例程将订阅/turtle1/pose话题，消息类型turtlesim::Pose

import rospy
import threading
import quaternions
import signal
import time
import sys
from geometry_msgs.msg import Twist, Pose, Quaternion, PoseStamped, PoseWithCovarianceStamped
from math import pi
from nav_msgs.msg import Odometry
from roborts_msgs.msg import GimbalAngle, Target3D
from roborts_msgs.msg import GameResult, GameStatus, GameZone, GameArry, RobotStatus, RoboShoot, GameRobotHP, GameZoneArray, GameRobtBullet

def quit(signum, frame):
    print ('stop fusion')
    sys.exit(0)

def Callback1(msg):
    global euler_angle, n, init_yaw, last_dtec, this_detec, rec_num
    qa = msg.pose.pose.orientation
    a = quaternions.Quaternion(qa.w,qa.x,qa.y,qa.z)
    euler_angle = quaternions.Quaternion.get_euler(a)
    # if this_detec==1 and last_dtec==0:
    if rec_num>40:
        print('**********')
        init_yaw = euler_angle[2]
        this_detec = 0
        
    else:
    	this_detec = 1
       
    n+=1
    # euler_angle = euler_angle[::-1]
    # rospy.loginfo("odom  roll:%0.3f, pitch:%0.3f, , yaw:%0.3f", euler_angle[0]*180/pi, euler_angle[1]*180/pi,euler_angle[2]*180/pi)

def Callback2(msg):
    global z, last_dtec, this_detec, rec_num
    z = msg.pz
    last_dtec = this_detec
    if z==0:
        rec_num += 1
    else:
         rec_num = 0

def Callback3(msg):
    global angle_gim
    angle_gim = msg.yaw_angle
    # print(angle_gim)

def Callback4(msg):
    global dtec_gim_ang
    dtec_gim_ang.yaw_angle = msg.yaw_angle
    dtec_gim_ang.pitch_angle = msg.pitch_angle
    rospy.loginfo("&&&&&detected gimbal yaw : %0.2f deg&&&&&", (dtec_gim_ang.yaw_angle)*180/pi)
    rospy.loginfo("&&&&&detected gimbal pitch : %0.2f deg&&&&&", (dtec_gim_ang.pitch_angle)*180/pi)

def Callback5(msg):
    global chas_angular
    chas_angular = msg.angular.z

def Callback6(msg):
    global chas_pose
    chas_pose = msg
    rospy.loginfo("&&&&& chassis x : %0.2f , y : %0.2f , z : %0.2f , &&&&&", chas_pose.pose.position.x, chas_pose.pose.position.y, chas_pose.pose.position.z)

def calbac_Gamsta(msg):
    global gamStas, ReminTime
    gamStas = msg.game_status
    ReminTime = msg.remaining_time

def calbac_Rob_Shoot(msg):
    global freQuency,Speed
    freQeency = msg.frequency
    Speed = msg.speed

def calbac_GamRoHp(msg):
    global red1Hp, red2Hp, blue1Hp, blue2Hp
    red1Hp = msg.red1
    red2Hp = msg.red2
    blue1Hp = msg.blue1
    blue2Hp = msg.blue2

def calbac_Gam_Zon_Arry(msg):
    global Zon_arry
    Zon_arry = msg.zone

def calbac_Gam_RoBult(msg):
    global red1Bullt, red2Bullt, blue1Bullt, blue2Bullt
    red1Bullt = msg.red1
    red2Bullt = msg.red2
    blue1Bullt = msg.blue1
    blue2Bullt = msg.blue2

def Gimbal(rate):
    global z, euler_angle, n, init_yaw, angle_gim, dtec_gim_ang
    #设置循环的频率
    rate1 = rospy.Rate(rate)
    n1 = 0
    while True:
        # 创建一个Publisher，发布名为/cmd_gimbal_angle的topic
        gim_ang_pub = rospy.Publisher("/cmd_gimbal_angle",  GimbalAngle, queue_size=1)

        if n1==0:
            print('##########Gimbal Thread Active!##########')
        n1+=1
        if (not rospy.is_shutdown()) and (this_detec==1):
            
            gim_ang_msg = GimbalAngle()
            gim_ang_msg.yaw_mode = False
            gim_ang_msg.pitch_mode = False
            gim_ang_msg.yaw_angle = -(euler_angle[2]-init_yaw) + dtec_gim_ang.yaw_angle
            gim_ang_msg.pitch_angle = dtec_gim_ang.pitch_angle
            # if abs(gim_ang_msg.yaw_angle)>pi/2:
            #     print('999999999999999999999999999999999999999999999999999999')
            #     gim_ang_msg.yaw_angle  = gim_ang_msg.yaw_angle/abs(gim_ang_msg.yaw_angle)*pi/2
            # print('init yaw = %0.2f', init_yaw*180/pi)
            # 发布消息
            # gim_ang_pub.publish(gim_ang_msg)
            # rospy.loginfo("&&&&&Publsh gimbal angle command: %0.2f deg&&&&&", (gim_ang_msg.yaw_angle)*180/pi)
        else:
            # pass
            gim_ang_msg = GimbalAngle()
            gim_ang_msg.yaw_mode = False
            gim_ang_msg.yaw_angle = 0
            # gim_ang_pub.publish(gim_ang_msg)
            # rospy.loginfo("&&&&&Publsh gimbal angle command: %0.2f deg&&&&&", gim_ang_msg.yaw_angle*180/pi)

        # 按照循环频率延时
        rate1.sleep()       

def Chasis(rate):
    global z, euler_angle, n, init_yaw, angle_gim, last_dtec, this_detec, con_num, goal_angle, chas_angular
    rate2 = rospy.Rate(rate)
    n2 = 0
    flag = -1
    while True:
        # 创建一个Publisher，发布名为/cmd_vel的topic
        chis_ang_pub = rospy.Publisher('/cmd_vel', Twist, queue_size=1) 
        chis_pos_pub = rospy.Publisher('/move_base_simple/goal', PoseStamped, queue_size=1) 
        

        if n2 == 0:
            print('##########Chasis Thread Active!##########')
        pos_cmd = PoseStamped()
        pos_cmd.header.frame_id = 'map'
        pos_cmd.pose.position.x = 3
        pos_cmd.pose.position.y = 3
        pos_cmd.pose.position.z = 0
        chis_pos_pub.publish(pos_cmd)
        print('##########Goal Pose Send!##########')
        n2+=1
        # if (not rospy.is_shutdown()) and (this_detec==1):
        #     kp = 0.5
        #     kd = 0
        #     # Initialize the movement command
        #     move_cmd = Twist()
        #     move_cmd.angular.z = kp*(init_yaw+goal_angle-euler_angle[2]) + kd*(0-chas_angular)
        #     chis_ang_pub.publish(move_cmd)
        #     # rospy.loginfo("****Publsh chasis angle command: %0.2f deg/s****", move_cmd.angular.z*180/pi)
        #     rospy.loginfo("**** chasis angle gap: %0.2f deg  Publsh chasis angle command: %0.2f deg/s****", (init_yaw+goal_angle-euler_angle[2])*180/pi,  move_cmd.angular.z*180/pi)
        #     if abs(init_yaw+goal_angle-euler_angle[2]) < 15*pi/180:
        #         goal_angle = goal_angle*(-1)
        #         print(goal_angle)

        # else:
            # angular_speed = 0.5
            # flag = 1
            # move_cmd = Twist()
            # move_cmd.angular.z = angular_speed*flag
            # chis_ang_pub.publish(move_cmd)
            # pass

        # 按照循环频率延时
        rate2.sleep()    

def pose_subscriber():
	# ROS节点初始化
    rospy.init_node('gimbal_control', anonymous=True)

	# 创建一个Subscriber，订阅名为/odom的topic，注册回调函数Callback1
    rospy.Subscriber("/odom", Odometry, Callback1, queue_size=30)
    # 创建一个Subscriber，订阅名为/turtle1/pose的topic，注册回调函数Callback2
    rospy.Subscriber("/target_position", Target3D, Callback2, queue_size=30)  
    # 创建一个Subscriber，订阅名为/turtle1/pose的topic，注册回调函数Callback2
    rospy.Subscriber("/cmd_gimbal_angle",  GimbalAngle, Callback3, queue_size=1) 
    # 创建一个Subscriber，订阅名为/turtle1/pose的topic，注册回调函数Callback2
    rospy.Subscriber("/cmd_gimbal_angle_c",  GimbalAngle, Callback4, queue_size=100)
    # 创建一个Subscriber，订阅名为/turtle1/pose的topic，注册回调函数Callback2
    rospy.Subscriber('/cmd_vel', Twist, Callback5, queue_size=1) 
    # 创建一个Subscriber，订阅名为/turtle1/pose的topic，注册回调函数Callback2
    rospy.Subscriber('/amcl_pose', PoseWithCovarianceStamped, Callback6, queue_size=1)
    # 创建一 
    rospy.Subscriber('/game_status', GameZone, calbac_Gamsta, queue_size = 30)

    rospy.Subscriber('/robot_shoot', RoboShoot, calbac_Rob_Shoot, queue_size = 30)

    rospy.Subscreiber('/game_robot_hp', GameRobotHP, calbac_GamRoHp, queue_size = 30)

    rospy.Subscreiber('/game_zone_array_status', GameZoneArray, calbac_Gam_Zon_Arry, queue_size = 30)

    rospy.Subscreiber('/game_robot_bullet', GameRobtBullet, calbac_Gam_RoBult, queue_size = 30)


    rate_send1 = 50
    rate_send2 = 50

    vehicleThreads = []
    T1 = threading.Thread(target=Gimbal,args=(rate_send1,))
    vehicleThreads.append(T1)
    T2 = threading.Thread(target=Chasis,args=(rate_send2,))
    vehicleThreads.append(T2)
    for t in vehicleThreads:
        t.start()

	# 循环等待回调函数
    rospy.spin()

if __name__ == '__main__':
    n = 0
    z = 0
    euler_angle = [0,0,0]
    init_yaw = 0
    last_dtec = 0
    this_detec = 0
    rec_num = 0
    con_num = 0
    angle_gim = 0
    goal_angle = 9*pi/20
    chas_angular = 0
    dtec_gim_ang = GimbalAngle()
    chas_pose = PoseWithCovarianceStamped()
    signal.signal(signal.SIGINT, quit)                                
    signal.signal(signal.SIGTERM, quit)

    try:
        pose_subscriber()
        
    except rospy.ROSInterruptException:
        pass
